//
//  local_mapper.h
// 
/*
   Copywrite 2013. All rights reserved.
   CyPhy Lab, https://wiki.qut.edu.au/display/cyphy
   Queensland University of Technology
   Brisbane, Australia

Author: Timothy Morris

Redistribution and use in source and binary forms, with or 
without modification, are permitted provided that the following 
conditions are met:

Redistributions of source code must retain the above copyright notice, 
this list of conditions and the following disclaimer. 

Redistributions in binary form must reproduce the above copyright notice, 
this list of conditions and the following disclaimer in the documentation 
and/or other materials provided with the distribution. 

Neither the name of the Queensland University of Technology, CyPhy Lab nor the names of 
the contributors may be used to endorse or promote products derived 
from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL 
THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 
THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
OF SUCH DAMAGE.
 */ 

#ifndef _local_mapper_h
#define _local_mapper_h

#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
#include <slam_backend/NodeAdded.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose2D.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <local_mapper/LocalizedScan.h>
#include <local_mapper/GetLocalMap.h>
#include <local_mapper/GetContainedNodes.h>
#include <slam_backend/GetNodesWithinX.h>
#include <slam_backend/GetNodePose.h>
#include <occupancy_grid_utils/ray_tracer.h>
#include <mongo_ros/message_collection.h>
#include <mongo_ros/exceptions.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/OccupancyGrid.h>
#include <boost/thread.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/ros/conversions.h>
//#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <laser_geometry/laser_geometry.h>
#include <algorithm>
#include <pcl/features/pfh.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/features/fpfh_omp.h>
#include <pcl/registration/correspondence_rejection_one_to_one.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/transforms.h>
#include <pcl/registration/transformation_estimation_svd.h>
//#include <pcl/registration/transformation_estimation_2D.h>
#include <pcl/registration/transformation_estimation_point_to_plane.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/visualization/cloud_viewer.h>
//#include <sparse_stereo/featurePoint.h>

#include <ndt_matcher_p2d.h>
#include <ndt_matcher_d2d.h>
#include <ndt_map.h>

namespace mr = mongo_ros;
namespace gu = occupancy_grid_utils;

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef pcl::PointCloud<pcl::PointXYZRGB>::Ptr PointCloudPtr;
typedef mongo_ros::MessageWithMetadata<sensor_msgs::LaserScan>::ConstPtr LaserWithMetadata;
typedef mongo_ros::MessageWithMetadata<sensor_msgs::PointCloud2>::ConstPtr PointcloudWithMetadata;
typedef mongo_ros::MessageWithMetadata<nav_msgs::Odometry>::ConstPtr OdometryWithMetadata;
typedef mongo_ros::MessageWithMetadata<nav_msgs::OccupancyGrid>::ConstPtr GridsWithMetadata;
typedef mongo_ros::MessageWithMetadata<PointCloud>::ConstPtr MapCloudWithMetadata;

class LocalMapper
{
private:
	// Ros Stuff
	ros::NodeHandle nh_;
	
	//// Subscriptions
	ros::Subscriber laser_sub_;
	ros::Subscriber pointcloud_sub_;
	ros::Subscriber odom_sub_;
	ros::Subscriber node_added_sub_;
	tf::TransformListener *tf_listener_;
	
	//// Services
	ros::ServiceServer switch_service_;
	ros::ServiceServer get_contained_nodes_service_;
	// Clients
	ros::ServiceClient neighbouring_node_client_;
	ros::ServiceClient node_pose_client_;
	
	//// Publications
	ros::Publisher map_pub_;
	tf::TransformBroadcaster tf_broadcaster_;
	boost::shared_ptr<ros::Publisher> pc_pub_;
	
	//// Data
	nav_msgs::OccupancyGrid::ConstPtr local_map_;
	// Graph
	// Interior
	std::vector<int> interior_node_ids_;
	std::vector<geometry_msgs::Pose2D> interior_node_poses_;
	std::vector<int> interior_sequence_idx_;
	// Exterior
	std::vector<int> exterior_node_ids_;
	std::vector<geometry_msgs::Pose2D> exterior_node_poses_;
	std::vector<int> exterior_sequence_idx_;
	
	tf::Transform local_transform_;
	
	//// Mongo
	// Cache
	mongo_ros::MessageCollection<nav_msgs::Odometry> odomCache_;
	mongo_ros::MessageCollection<sensor_msgs::LaserScan> laserCache_;
	mongo_ros::MessageCollection<sensor_msgs::PointCloud2> pointcloudCache_;
	// Store
	mongo_ros::MessageCollection<nav_msgs::Odometry> odomDB_;
	mongo_ros::MessageCollection<sensor_msgs::LaserScan> laserDB_;
	mongo_ros::MessageCollection<sensor_msgs::PointCloud2> pointcloudDB_;
	mongo_ros::MessageCollection<nav_msgs::OccupancyGrid> occupancyGridDB_;
	mongo_ros::MessageCollection<PointCloud> localcloudDB_;
	
	//// Params
	// Topics
	std::string map_topic_;
	std::string laser_topic_;
	std::string pointcloud_topic_;
	std::string odom_topic_;
	std::string node_added_topic_;
	// Services
	std::string switch_srv_name_;
	std::string neighbour_srv_name_;
	std::string pose_srv_name_;
	// Frames
	std::string local_frame_;
	std::string global_frame_;
	// Database
	std::string db_name_;
	std::string cache_name_;
	
	// Switches
	bool online_mode_;
	bool align_border_segments_;
	bool use_ndt_;
	bool use_pointclouds_;
	bool use_single_sequence_;
	bool use_existing_data_;
	
	// Map Properties
	double map_width_;
	double map_height_;
	double resolution_;
	double origin_x_;
	double origin_y_;
	// Data Used
	double interior_limit_;
	double exterior_limit_;
	int seq_sample_;
	int cache_window_;
	int datum_per_node_;
	tf::StampedTransform node_pose_;
	
	//// Boost
	boost::mutex m_lock_;
	
	//// Occupancy Params
	double occupancy_threshold_;
	double max_distance_;
	double min_pass_through_;
	bool save_map_files_;
	
	//// Registration of Point Clouds
	// NDT
	double ndt_StepSize_;
	double ndt_Resolution_;
	double ndt_TransformationEpsilon_;
	int ndt_MaximumIterations_;
	double ndt_cloud_threshold_;
	double ndt_sequence_threshold_;
	// FPFH
	bool register_segments_;
	double feature_radius_;
	double normal_radius_;
	// Resolution
	double downsample_res_;
	double keypoint_res_;
	// Correspondence
	int min_corres_;
	int node_neigh_;
	int sequence_gap_;
	double inlier_threshold_;
	double correspondence_distance_;
	// Debuging Params
	bool save_pcd_files_;
	bool visualise_clouds_;
	bool visualise_sequences_;
	
public:
	// Constructor
	LocalMapper(std::string &db_name, std::string &cache_name);
	
	//// Map Construction
	bool NewLocalMap(local_mapper::GetLocalMap::Request &req,
					 local_mapper::GetLocalMap::Response &res);
	bool BuildMap(int nodeID);
	// Database Callbacks
	void laserCB(const sensor_msgs::LaserScan& msg);
	void pointcloudCB(const sensor_msgs::PointCloud2& msg);
	void odomCB(const nav_msgs::Odometry& msg);
	void nodeAddedCB(const slam_backend::NodeAdded& msg);
	// Manipulators and Extractors
	void repositionCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in,
						 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out);
	
	
	//// Cloud Correspondences
	Eigen::Matrix4f matchClouds(pcl::PointCloud<pcl::PointXYZRGB>::Ptr target, pcl::PointCloud<pcl::PointXYZRGB>::Ptr source);
	bool ndtClouds(pcl::PointCloud<pcl::PointXYZRGB>::Ptr target, pcl::PointCloud<pcl::PointXYZRGB>::Ptr source, double threshold, bool vis);

	void getKeyPointsCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr input,
						   pcl::PointCloud<pcl::PointXYZI>::Ptr output, double leafSize);
	
	void extractDescriptors(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudIN,
				pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints,
				pcl::PointCloud<pcl::FPFHSignature33>::Ptr features);
	
	void findCorrespondences(pcl::PointCloud<pcl::FPFHSignature33>::Ptr source,
							 pcl::PointCloud<pcl::FPFHSignature33>::Ptr target,
							 std::vector<int>& correspondences);
	
	void filterCorrespondences(pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints_,
							   pcl::PointCloud<pcl::PointXYZI>::Ptr target_keypoints_,
							   std::vector<int>& source2target_, std::vector<int>& target2source_,
							   pcl::CorrespondencesPtr correspondences_);
	
	//// Utilities
	// Publishers
	//void publishThread();
	void publishTransform();
	// Service Callers
	bool getContainedNodes(local_mapper::GetContainedNodes::Request &req,
						   local_mapper::GetContainedNodes::Response &res);
	bool GetNodeDataAround(int nodeID);
	// Filters
	void passfilterCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in,
						 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out,
						 float min, float max, std::string fieldName = "x");
	
	void removeOutliers(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in,
						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out);
	
	void downSampleCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input,
						 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output,
						 double leafSize);
	
	void changeColor(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input, int r,
					 int g, int b);
	
	// Debugging
	void showCorrespondingPairs(pcl::visualization::PCLVisualizer &vis,
								pcl::CorrespondencesPtr &CorrespondingPairs,
								pcl::PointCloud<pcl::PointXYZI>::Ptr &cloudOriginKeypoints,
								pcl::PointCloud<pcl::PointXYZI>::Ptr &keypointDisplay);
	
	//// Other
	void fillGridDB();		
	Eigen::Matrix4f determineInitialTransformation(pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints_,
																pcl::PointCloud<pcl::PointXYZI>::Ptr target_keypoints_,
																pcl::CorrespondencesPtr correspondences_);	
	
	double getDistance(pcl::PointXYZI& p1, pcl::PointXYZI& p2);
};


#endif
